﻿/// 本例是测试 两个不同角度拍摄CT图，然后转成3D坐标

#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <Eigen/Dense>

// opencv
#include <opencv2/core/eigen.hpp>

// vtk
#include <vtkLine.h>
#include <vtkPlane.h>

#include <iostream>

using namespace std;
using namespace cv;

/// 填充数据
/// 一个H可以生成两行矩阵的值
static void fillVij(double *p1, double *p2, Mat &H1)
{
    p1[0] = H1.at<double>(0,0) * H1.at<double>(0,1);
    p1[1] = H1.at<double>(0,0) * H1.at<double>(1,1) + H1.at<double>(1,0) * H1.at<double>(0,1);
    p1[2] = H1.at<double>(1,0) * H1.at<double>(1,1);
    p1[3] = H1.at<double>(2,0) * H1.at<double>(0,1) + H1.at<double>(0,0) * H1.at<double>(2,1);
    p1[4] = H1.at<double>(2,0) * H1.at<double>(1,1) + H1.at<double>(1,0) * H1.at<double>(2,1);
    p1[5] = H1.at<double>(2,0) * H1.at<double>(2,1);

    p2[0] = H1.at<double>(0,0) * H1.at<double>(0,0) - (H1.at<double>(0,1) * H1.at<double>(0,1));
    p2[1] = H1.at<double>(0,0) * H1.at<double>(1,0) + H1.at<double>(1,0) * H1.at<double>(0,0)
            - (H1.at<double>(0,1) * H1.at<double>(1,1) + H1.at<double>(1,1) * H1.at<double>(0,1) );
    p2[2] = H1.at<double>(1,0) * H1.at<double>(1,0) - ( H1.at<double>(1,1) * H1.at<double>(1,1) );
    p2[3] = H1.at<double>(2,0) * H1.at<double>(0,0) + H1.at<double>(0,0) * H1.at<double>(2,0)
            - (H1.at<double>(2,1) * H1.at<double>(0,1) + H1.at<double>(0,1) * H1.at<double>(2,1));
    p2[4] = H1.at<double>(2,0) * H1.at<double>(1,0) + H1.at<double>(1,0) * H1.at<double>(2,0)
            - (H1.at<double>(2,1) * H1.at<double>(1,1) + H1.at<double>(1,1) * H1.at<double>(2,1));
    p2[5] = H1.at<double>(2,0) * H1.at<double>(2,0) - (H1.at<double>(2,1) * H1.at<double>(2,1));
}

/// 单应矩阵转化
static void homegraphy()
{
    // 实际坐标，投影到uv平面坐标
    vector<Point2f> objPoints1 = {
        {0, 0},
        {100, 0},
        {100, 100},
        {0, 100}
    };

    vector<Point2f> uvPoints1 = {
        {200+0, 200+0},
        {200+200, 200+0},
        {200+200, 200+200},
        {200+0, 200+200}
    };

    Mat H1 = findHomography(objPoints1, uvPoints1);
    cout << "H1:\n" << H1 << endl;

    vector<Point2f> objPoints2 = {
        {0, 0},
        {100, 0},
        {100, 100},
        {0, 100}
    };

    vector<Point2f> uvPoints2 = {
        {200+0, 200+0},
        {200+125, 200+0},
        {200+125, 200+125},
        {200+0, 200+125}
    };

    Mat H2 = findHomography(objPoints2, uvPoints2);
    cout << "H2:\n" << H2<< endl;

    const float y_ = 10.0*sqrt(136.0);
    vector<Point2f> objPoints3 = {
        {0, 0},
        {100, 0},
        {100, y_},
        {0, y_}
    };

    vector<Point2f> uvPoints3 = {
        {200+0, 200+0},
        {200+200, 200+0},
        {200+125, 200+125},
        {200+0, 200+125}
    };

    Mat H3 = findHomography(objPoints3, uvPoints3);
    cout << "H3:\n" << H3<< endl;

    // AX=0
    Mat A = Mat(6,6,CV_64F);
//    double zero[6] = {0, 0, 0, 0, 0, 0};
//    Mat B = Mat(6,1,CV_64F, zero);

    fillVij(A.ptr<double>(0), A.ptr<double>(1), H1);
    fillVij(A.ptr<double>(2), A.ptr<double>(3), H2);
    fillVij(A.ptr<double>(4), A.ptr<double>(5), H3);


    cout << "A:\n" << A << endl;

    Mat B = Mat(6,1,CV_64F);
    SVD::solveZ(A, B);

    cout << "B: \n" << B << endl;

    vector<double> b = B.reshape(1, 1);
    cout << "b:" << b[0] << "," << b[1] << ", " << b[2] << endl;
    double v0 = (b[1]*b[3] - b[0]*b[4])/(b[0]*b[2] - b[1]*b[1]);
    double  gama = b[5] - (b[3]*b[3] + v0*(b[1]*b[3]-b[0]*b[4]))/b[0];
    double alpha = sqrt(gama/b[0]);
    double beta = sqrt(gama*b[0]/(b[0]*b[2] - b[1]*b[1]));
    double lamda = - b[1]*alpha*alpha*beta/gama;
    double u0 = lamda*v0/alpha - b[3]*alpha*alpha/gama;

    cout << "alpha: " << alpha << ", beta: " << beta << " gama: " << gama << " u0:" << u0 << ", v0: " << v0 << endl;

}


/// 相机模型
static void getCameraModel(vector<Point2f> &src1_C1, vector<Point2f> &dst1_C1,
                           vector<Point2f> &src2_C1, vector<Point2f> &dst2_C1,
                           vector<Point2f> &src1_C2, vector<Point2f> &dst1_C2,
                           vector<Point2f> &src2_C2, vector<Point2f> &dst2_C2,
                           Eigen::Vector3d &selPtProj_C1, Eigen::Vector3d &selPtProj_C2,
                           Eigen::Isometry3d &T_C2toC1, Eigen::Isometry3d &T_C1toC2,
                           const double offsetX = 10, const double offsetZ = 10)
{
//    double dataK[9] = {
//        10240, 0.0, -512,
//        0, 10240, 512,
//        0, 0, 1
//    };
    // Mat K = Mat(3,3,CV_64F, dataK);
    Eigen::Matrix3d K;
    K <<    5120, 0.0, 512,
            0, 5120, 512,
            0, 0, 1;
    cout << "K:\n" << K << endl;

    // 模拟两个空间平行面的数据
    // 如果投影面和空间平面不平行，那么乘以一个旋转矩阵就可以得到新的模拟数据
    std::vector<Eigen::Vector3d> objPts1 = {
        {50, -50, 500},
        {-50, -50, 500},
        {-50, 50, 500},
        {50, 50, 500}
    };
    std::vector<Eigen::Vector3d> objPts2 = {
        {50, -50, 600},
        {-50, -50, 600},
        {-50, 50, 600},
        {50, 50, 600}
    };


    cout << "plane 1 in C1:" << endl;
    for (uint i = 0; i < objPts1.size(); i++) {
         Eigen::Vector3d pt = K*objPts1[i];
         pt = pt/pt[2];

         cout << "    " << pt.transpose() << endl;
         Point2f p2f(objPts1[i][0], objPts1[i][1]);
         src1_C1.push_back(p2f);
         dst1_C1.push_back(Point2f(pt[0],pt[1]));
    }

    cout << "plane 2 in C2:" << endl;
    for (uint i = 0; i < objPts2.size(); i++) {
         Eigen::Vector3d pt = K*objPts2[i];
         pt = pt/pt[2];

         cout << "    " << pt.transpose() << endl;
         Point2f p2f(objPts2[i][0], objPts2[i][1]);
         src2_C1.push_back(p2f);
         dst2_C1.push_back(Point2f(pt[0],pt[1]));
    }

    Eigen::Vector3d verifyP = {50, 40, 650};
    cout << "verifyP in C1:" << verifyP.transpose() << endl;
    selPtProj_C1 = K*verifyP; selPtProj_C1 = selPtProj_C1/selPtProj_C1[2];
    cout << "verifyP in C1(project): " << selPtProj_C1.transpose() << endl;

    // C1 和 C2 空间位置
    Eigen::AngleAxisd angleA_C1toC2(-90.0/180*CV_PI, Eigen::Vector3d::UnitY());
    // cout << "R:" << angleA_C1toC2.toRotationMatrix() << endl;

    T_C2toC1 = Eigen::Isometry3d::Identity();
    //Eigen::Isometry3d T_pre = Eigen::Isometry3d::Identity();
    //T_pre.pretranslate(Eigen::Vector3d(0, 0, 500));
    T_C2toC1.rotate(angleA_C1toC2.toRotationMatrix());
    T_C2toC1.pretranslate(Eigen::Vector3d(650, 0, 600));
    //T_C2toC1 = T_pre * T_C2toC1;

    T_C1toC2 = T_C2toC1.inverse();

    cout << "T_C2toC1:\n" << T_C2toC1.matrix() << endl;
    cout << "T_C1toC2:\n" << T_C1toC2.matrix() << endl;


    // cout << "T_C1toC2*(0, 0, 600):\n" << T_C1toC2* Eigen::Vector3d(0, 0, 600) << endl;


    // 模拟两个空间平行面的数据
    // 如果投影面和空间平面不平行，那么乘以一个旋转矩阵就可以得到新的模拟数据
    //const double offsetX = 0;
    //const double offsetZ = 0;
    std::vector<Eigen::Vector3d> objPts1_C2 = {
        {50+offsetX, -50,  500 + offsetZ },
        {-50+offsetX, -50, 500 + offsetZ},
        {-50+offsetX, 50,  500 + offsetZ},
        {50+offsetX, 50,   500 + offsetZ}
    };
    std::vector<Eigen::Vector3d> objPts2_C2 = {
        {50+offsetX,  -50, 600+offsetZ},
        {-50+offsetX, -50, 600+offsetZ},
        {-50+offsetX, 50,  600+offsetZ},
        {50+offsetX,  50,  600+offsetZ}
    };

    cout << "plane 1 in  C2:" << endl;
    for (uint i = 0; i < objPts1.size(); i++) {
         Eigen::Vector3d pt = K*objPts1_C2[i];
         pt = pt/pt[2];

         cout << "    " << pt.transpose() << endl;
         Point2f p2f(objPts1_C2[i][0], objPts1_C2[i][1]);
         src1_C2.push_back(p2f);
         dst1_C2.push_back(Point2f(pt[0],pt[1]));
    }

    cout << "plane 2 in C2:" << endl;
    for (uint i = 0; i < objPts2.size(); i++) {
         Eigen::Vector3d pt = K*objPts2_C2[i];
         pt = pt/pt[2];

         cout << "    " << pt.transpose() << endl;
         Point2f p2f(objPts2_C2[i][0], objPts2_C2[i][1]);
         src2_C2.push_back(p2f);
         dst2_C2.push_back(Point2f(pt[0],pt[1]));
    }

    Eigen::Vector3d verifyP_C2 = T_C1toC2 * verifyP;
    cout << "verifyP in C2: " << verifyP_C2.transpose() << endl;
    selPtProj_C2 = K*verifyP_C2; selPtProj_C2 = selPtProj_C2/selPtProj_C2[2];
    cout << "verifyP in C2(project): " << selPtProj_C2.transpose() << endl;
}


/// 单应矩阵转化
static void homegraphy2()
{
    vector<Point2f> src1_C1;
    vector<Point2f> dst1_C1;
    vector<Point2f> src2_C1;
    vector<Point2f> dst2_C1;
    vector<Point2f> src1_C2;
    vector<Point2f> dst1_C2;

    vector<Point2f> src2_C2;
    vector<Point2f> dst2_C2;
    Eigen::Vector3d selPtProj_C1;
    Eigen::Vector3d selPtProj_C2;

    Eigen::Isometry3d T_C2toC1, T_C1toC2;
    getCameraModel(src1_C1, dst1_C1, src2_C1, dst2_C1, src1_C2, dst1_C2, src2_C2, dst2_C2, selPtProj_C1, selPtProj_C2, T_C2toC1, T_C1toC2);

    cout << "==============================" << endl;
    cout << "src1_C1: " << src1_C1 << endl;
    cout << "dst1_C1: " << dst1_C1 << endl;
    cout << "src2_C1: " << src2_C1 << endl;
    cout << "dst2_C1: " << dst2_C1 << endl;
    cout << "selPtProj_C1: " << selPtProj_C1.transpose() << endl;

    vector<Point2f> src3_C1;
    src3_C1.push_back(src1_C1[0]); src3_C1.push_back(src1_C1[1]);src3_C1.push_back(src2_C1[2]); src3_C1.push_back(src2_C1[3]);
    vector<Point2f> dst3_C1;
    dst3_C1.push_back(dst1_C1[0]); dst3_C1.push_back(src1_C1[1]);dst3_C1.push_back(dst2_C1[2]); dst3_C1.push_back(dst2_C1[3]);
    cout << "src3_C1: " << src3_C1 << endl;
    cout << "dst3_C1: " << dst3_C1 << endl;

    vector<Point2f> src4_C1;
    src4_C1.push_back(src1_C1[2]); src4_C1.push_back(src1_C1[3]);src4_C1.push_back(src2_C1[0]); src4_C1.push_back(src2_C1[1]);
    vector<Point2f> dst4_C1;
    dst4_C1.push_back(dst1_C1[2]); dst4_C1.push_back(src1_C1[3]);dst4_C1.push_back(dst2_C1[0]); dst4_C1.push_back(dst2_C1[1]);
    cout << "src4_C1: " << src4_C1 << endl;
    cout << "dst4_C1: " << dst4_C1 << endl;


    Mat H1 = getPerspectiveTransform(src1_C1, dst1_C1);
    cout << "H1 = " << H1 << endl;

    Mat H2 = getPerspectiveTransform(src2_C1, dst2_C1);
    cout << "H2 = " << H2 << endl;

    Mat H3 = getPerspectiveTransform(src3_C1, dst3_C1);
    cout << "H3 = " << H3 << endl;

    Mat H4 = getPerspectiveTransform(src4_C1, dst4_C1);
    cout << "H4 = " << H4 << endl;

    Mat H1_C2 = getPerspectiveTransform(src1_C2, dst1_C2);

    cout << "H1_C2 = " << H1_C2 << endl;

    Mat H2_C2 = getPerspectiveTransform(src2_C2, dst2_C2);
    cout << "H2_C2 = " << H2_C2 << endl;


    // AX=0
    Mat A = Mat(6,6,CV_64F);
//    double zero[6] = {0, 0, 0, 0, 0, 0};
//    Mat B = Mat(6,1,CV_64F, zero);

    fillVij(A.ptr<double>(0), A.ptr<double>(1), H1);
    fillVij(A.ptr<double>(2), A.ptr<double>(3), H4);
    fillVij(A.ptr<double>(4), A.ptr<double>(5), H3);
    //fillVij(A.ptr<double>(6), A.ptr<double>(7), H2_C2);


    cout << "A:\n" << A << endl;

    Mat B = Mat(6,1,CV_64F);
    SVD::solveZ(A, B);

    cout << "B: \n" << B << endl;

    vector<double> b = B.reshape(1, 1);
    cout << "b:" << b[0] << "," << b[1] << ", " << b[2] << endl;
    double v0 = (b[1]*b[3] - b[0]*b[4])/(b[0]*b[2] - b[1]*b[1]);
    double  gama = b[5] - (b[3]*b[3] + v0*(b[1]*b[3]-b[0]*b[4]))/b[0];
    double alpha = sqrt(gama/b[0]);
    double beta = sqrt(gama*b[0]/(b[0]*b[2] - b[1]*b[1]));
    double lamda = - b[1]*alpha*alpha*beta/gama;
    double u0 = lamda*v0/alpha - b[3]*alpha*alpha/gama;

    cout << "alpha: " << alpha << ", beta: " << beta << " gama: " << gama << " u0:" << u0 << ", v0: " << v0 << endl;

}

/// 相机模型
static void cameraModel()
{

    vector<Point2f> src1_C1;
    vector<Point2f> dst1_C1;
    vector<Point2f> src2_C1;
    vector<Point2f> dst2_C1;
    vector<Point2f> src1_C2;
    vector<Point2f> dst1_C2;

    vector<Point2f> src2_C2;
    vector<Point2f> dst2_C2;
    Eigen::Vector3d selPtProj_C1;
    Eigen::Vector3d selPtProj_C2;
    Eigen::Isometry3d T_C2toC1, T_C1toC2;
    getCameraModel(src1_C1, dst1_C1, src2_C1, dst2_C1, src1_C2, dst1_C2, src2_C2, dst2_C2, selPtProj_C1, selPtProj_C2, T_C2toC1, T_C1toC2);
}

static void ct2dTo3D()
{
    vector<Point2f> src1_C1;
    vector<Point2f> dst1_C1;
    vector<Point2f> src2_C1;
    vector<Point2f> dst2_C1;
    vector<Point2f> src1_C2;
    vector<Point2f> dst1_C2;

    vector<Point2f> src2_C2;
    vector<Point2f> dst2_C2;
    Eigen::Vector3d selPtProj_C1;
    Eigen::Vector3d selPtProj_C2;

    const double offsetX = 10;
    Eigen::Isometry3d T_C2toC1, T_C1toC2;
    getCameraModel(src1_C1, dst1_C1, src2_C1, dst2_C1, src1_C2, dst1_C2, src2_C2, dst2_C2, selPtProj_C1, selPtProj_C2, T_C2toC1, T_C1toC2, offsetX, 10);

    cout << "==============================" << endl;
    cout << "src1_C1: " << src1_C1 << endl;
    cout << "dst1_C1: " << dst1_C1 << endl;
    cout << "src2_C1: " << src2_C1 << endl;
    cout << "dst2_C1: " << dst2_C1 << endl;
    cout << "selPtProj_C1: " << selPtProj_C1.transpose() << endl;

    Mat H1 = getPerspectiveTransform(src1_C1, dst1_C1);

    cout << "H1 = " << H1 << endl;

    Mat H2 = getPerspectiveTransform(src2_C1, dst2_C1);
    cout << "H2 = " << H2 << endl;


    Eigen::Matrix3d H1e,H2e;
    cv2eigen(H1, H1e);
    cv2eigen(H2, H2e);
    cout << "H1e: " << H1e << endl;

    // 得到平行面上的两个点
    Eigen::Vector3d src1Pt1 = H1e.inverse()*selPtProj_C1;
    Eigen::Vector3d src2Pt1 = H2e.inverse()*selPtProj_C1;
    cout << "src1 pt1 in C1:" << src1Pt1.transpose() << ", " <<  src1Pt1.transpose()/500 << endl;
    cout << "src2 pt1 in C1:" << src2Pt1.transpose() << ", " <<  src2Pt1.transpose()/600 << endl;
    //cout << "selPt1:" << selPt1.transpose() << ", " <<  selPt1.transpose()/5120 << endl;

    cout << "----------------------------" << endl;
    cout << "src1_C2: " << src1_C2 << endl;
    cout << "dst1_C2: " << dst1_C2 << endl;
    cout << "src2_C2: " << src2_C2 << endl;
    cout << "dst2_C2: " << dst2_C2 << endl;
    cout << "selPtProj_C2: " << selPtProj_C2.transpose() << endl;


    Mat H1_C2 = getPerspectiveTransform(src1_C2, dst1_C2);

    cout << "H1_C2 = " << H1_C2 << endl;

    Mat H2_C2 = getPerspectiveTransform(src2_C2, dst2_C2);
    cout << "H2_C2 = " << H2_C2 << endl;

    Eigen::Vector3d src1Pt1_C2 = src1Pt1;  src1Pt1_C2[0] += offsetX;
    Eigen::Vector3d src2Pt1_C2 = src2Pt1;  src2Pt1_C2[0] += offsetX;


    Eigen::Matrix3d H1e_C2,H2e_C2;
    cv2eigen(H1_C2, H1e_C2);
    cv2eigen(H2_C2, H2e_C2);

    Eigen::Vector3d src1Pt1Proj_C2 = H1e_C2*src1Pt1_C2;
    Eigen::Vector3d src2Pt1Proj_C2 = H2e_C2*src2Pt1_C2;

    cout << "src1Pt1Proj_C2: " << src1Pt1Proj_C2.transpose() << endl;
    cout << "src2Pt1Proj_C2: " << src2Pt1Proj_C2.transpose() << endl;

    // 验证结果
    Eigen::Vector3d &proj_c2 = selPtProj_C2;

    double f1 = (proj_c2[0] - src1Pt1_C2[0]) / (proj_c2[1] - src1Pt1_C2[1]);
    double f2 = (proj_c2[0] - src2Pt1_C2[0]) / (proj_c2[1] - src2Pt1_C2[1]);
    cout << "factor in projection: " << f1 << ", " << f2 << endl;

}

static void c1c2Map()
{
    // C1 和 C2 空间位置
    Eigen::AngleAxisd angleA_C1toC2(-90.0/180*CV_PI, Eigen::Vector3d::UnitY());
    cout << "R:" << angleA_C1toC2.toRotationMatrix() << endl;

    Eigen::Isometry3d T_C2toC1 = Eigen::Isometry3d::Identity();
    //Eigen::Isometry3d T_pre = Eigen::Isometry3d::Identity();
    //T_pre.pretranslate(Eigen::Vector3d(0, 0, 500));
    T_C2toC1.rotate(angleA_C1toC2.toRotationMatrix());
    //T_C2toC1.pretranslate(Eigen::Vector3d(650, 0, 600));
    //T_C2toC1 = T_pre * T_C2toC1;

    Eigen::Isometry3d T_C1toC2 = T_C2toC1.inverse();

    cout << "T_C2toC1:\n" << T_C2toC1.matrix() << endl;
    cout << "T_C1toC2:\n" << T_C1toC2.matrix() << endl;

    cout << "C2 to C1: " << T_C2toC1 * Eigen::Vector3d(38.4615, 30.7692, 500) << endl;
    cout << "C2 to C1: " << T_C2toC1 * Eigen::Vector3d(36.9231, 37.5385, 600) << endl;
}

static void ct2dTo3D_geo()
{
    vector<Point2f> src1_C1;
    vector<Point2f> dst1_C1;
    vector<Point2f> src2_C1;
    vector<Point2f> dst2_C1;
    vector<Point2f> src1_C2;
    vector<Point2f> dst1_C2;

    vector<Point2f> src2_C2;
    vector<Point2f> dst2_C2;
    Eigen::Vector3d verifyPtProj_C1;
    Eigen::Vector3d verifyPtProj_C2;

    const double offsetX = 10;
    const double offsetZ = 10;
    Eigen::Isometry3d T_C2toC1, T_C1toC2;
    getCameraModel(src1_C1, dst1_C1, src2_C1, dst2_C1, src1_C2, dst1_C2, src2_C2, dst2_C2, verifyPtProj_C1, verifyPtProj_C2, T_C2toC1, T_C1toC2, offsetX, offsetZ);

    cout << "==============================" << endl;
    cout << "src1_C1: " << src1_C1 << endl;
    cout << "dst1_C1: " << dst1_C1 << endl;
    cout << "src2_C1: " << src2_C1 << endl;
    cout << "dst2_C1: " << dst2_C1 << endl;
    cout << "verifyPtProj_C1: " << verifyPtProj_C1.transpose() << endl;

    cout << "src1_C2: " << src1_C2 << endl;
    cout << "dst1_C2: " << dst1_C2 << endl;
    cout << "src2_C2: " << src2_C2 << endl;
    cout << "dst2_C2: " << dst2_C2 << endl;
    cout << "verifyPtProj_C2: " << verifyPtProj_C2.transpose() << endl;

    cout << "-----------C1 H----------------" << endl;
    Mat H1 = getPerspectiveTransform(src1_C1, dst1_C1);
    cout << "H1 = " << H1 << endl;

    Mat H2 = getPerspectiveTransform(src2_C1, dst2_C1);
    cout << "H2 = " << H2 << endl;

    Eigen::Matrix3d H1e,H2e;
    cv2eigen(H1, H1e);
    cv2eigen(H2, H2e);

    // 选取plane1 上的两点 src1_C1[0], src1_C1[1], 对应的
    Eigen::Vector3d selPt1_plane1 = {src1_C1[0].x, src1_C1[0].y, 1};
    Eigen::Vector3d selPt2_plane1 = {src1_C1[2].x, src1_C1[2].y, 1};
    cout << "selPt1 in plane1:" << selPt1_plane1.transpose()  << endl;
    cout << "selPt2 in plane1:" << selPt2_plane1.transpose()  << endl;

    // 计算UV平面的 selPt1_uv1, selPt2_uv2
    Eigen::Vector3d selPt1_uv1 = H1e*selPt1_plane1;
    Eigen::Vector3d selPt2_uv1 = H1e*selPt2_plane1;

    cout << "selPt1 in uv1:" << selPt1_uv1.transpose()  << endl;
    cout << "selPt2 in uv1:" << selPt2_uv1.transpose()  << endl;

    // 计算plane2 上的 selPt1_plane2, selPt2_plane2
    Eigen::Vector3d selPt1_plane2 = H2e.inverse()*selPt1_uv1;
    Eigen::Vector3d selPt2_plane2 = H2e.inverse()*selPt2_uv1;

    cout << "selPt1 in plane2:" << selPt1_plane2.transpose()  << endl;
    cout << "selPt2 in plane2:" << selPt2_plane2.transpose()  << endl;

    // 验证的点, 转成空间坐标
    Eigen::Vector3d verifyPt_plane1_C1 = H1e.inverse()*verifyPtProj_C1; verifyPt_plane1_C1[2] = 500;
    Eigen::Vector3d verifyPt_plane2_C1 = H2e.inverse()*verifyPtProj_C1; verifyPt_plane2_C1[2] = 600;
    cout << "verifyPt_plane1_C1:" << verifyPt_plane1_C1.transpose() << endl;
    cout << "verifyPt_plane2_C1:" << verifyPt_plane2_C1.transpose() << endl;

    double closestPt1[3], closestPt2[3];
    double t1, t2;
    selPt1_plane1[2] = 500;
    selPt2_plane1[2] = 500;
    selPt1_plane2[2] = 600;
    selPt2_plane2[2] = 600;
    vtkLine::DistanceBetweenLines(selPt1_plane2.data(), selPt1_plane1.data(),
                                  selPt2_plane2.data(), selPt2_plane1.data(),
                                  closestPt1, closestPt2, t1, t2);

    cout << "closestPt1: " << closestPt1[0] << ", " << closestPt1[1] << ", " << closestPt1[2] << endl;
    cout << "closestPt2: " << closestPt2[0] << ", " << closestPt2[1] << ", " << closestPt2[2] << endl;

    cout << "-----------C2 H----------------" << endl;
    Mat H1_C2 = getPerspectiveTransform(src1_C2, dst1_C2);
    cout << "H1_C2 = " << H1_C2 << endl;

    Mat H2_C2 = getPerspectiveTransform(src2_C2, dst2_C2);
    cout << "H2_C2 = " << H2_C2 << endl;

    Eigen::Matrix3d H1e_C2,H2e_C2;
    cv2eigen(H1_C2, H1e_C2);
    cv2eigen(H2_C2, H2e_C2);

    // 选取plane1 上的两点 src1_C2[0], src1_C2[1], 对应的
    Eigen::Vector3d selPt1_plane1_C2 = {src1_C2[0].x, src1_C2[0].y, 1};
    Eigen::Vector3d selPt2_plane1_C2 = {src1_C2[2].x, src1_C2[2].y, 1};
    cout << "selPt1 in plane1:" << selPt1_plane1_C2.transpose()  << endl;
    cout << "selPt2 in plane1:" << selPt2_plane1_C2.transpose()  << endl;

    // 计算UV平面的 selPt1_uv1, selPt2_uv2
    Eigen::Vector3d selPt1_uv2 = H1e_C2*selPt1_plane1_C2;
    Eigen::Vector3d selPt2_uv2 = H1e_C2*selPt2_plane1_C2;

    cout << "selPt1 in uv2:" << selPt1_uv2.transpose()  << endl;
    cout << "selPt2 in uv2:" << selPt2_uv2.transpose()  << endl;

    // 计算plane2 上的 selPt1_plane2, selPt2_plane2
    Eigen::Vector3d selPt1_plane2_C2 = H2e_C2.inverse()*selPt1_uv2;
    Eigen::Vector3d selPt2_plane2_C2 = H2e_C2.inverse()*selPt2_uv2;

    cout << "selPt1 in plane2:" << selPt1_plane2_C2.transpose()  << endl;
    cout << "selPt2 in plane2:" << selPt2_plane2_C2.transpose()  << endl;

    {
        double closestPt1_C2[3], closestPt2_C2[3];
        double t1, t2;
        selPt1_plane1_C2[2] = 500 + offsetZ;
        selPt2_plane1_C2[2] = 500 + offsetZ;
        selPt1_plane2_C2[2] = 600 + offsetZ;
        selPt2_plane2_C2[2] = 600 + offsetZ;
        vtkLine::DistanceBetweenLines(selPt1_plane2_C2.data(), selPt1_plane1_C2.data(),
                                      selPt2_plane2_C2.data(), selPt2_plane1_C2.data(),
                                      closestPt1_C2, closestPt2_C2, t1, t2);

        cout << "closestPt1: " << closestPt1_C2[0] << ", " << closestPt1_C2[1] << ", " << closestPt1_C2[2] << endl;
        cout << "closestPt2: " << closestPt2_C2[0] << ", " << closestPt2_C2[1] << ", " << closestPt2_C2[2] << endl;
    }

    // 验证的点, 转成空间坐标
    Eigen::Vector3d verifyPt_plane1_C2 = T_C1toC2 * verifyPt_plane1_C1;
    Eigen::Vector3d verifyPt_plane2_C2 = T_C1toC2 * verifyPt_plane2_C1;
    cout << "verifyPt_plane1_C2:" << verifyPt_plane1_C2.transpose() << endl;
    cout << "verifyPt_plane2_C2:" << verifyPt_plane2_C2.transpose() << endl;

    {
        // 找到和面 plane1 相交的点
        double p1_C2[3] = {0, 0, 0};
        double n[3] = {0, 0, 1};
        double plane1_p0_C2[3] = {0, 0, 500 + offsetZ};
        double t1_C2, t2_C2;
        double ptJD1_plane1_C2[3], ptJD2_plane1_C2[3];
        vtkPlane::IntersectWithLine(p1_C2, verifyPt_plane1_C2.data(), n, plane1_p0_C2, t1_C2, ptJD1_plane1_C2);
        vtkPlane::IntersectWithLine(p1_C2, verifyPt_plane2_C2.data(), n, plane1_p0_C2, t1_C2, ptJD2_plane1_C2);

        cout << "ptJD1_plane1_C2: " << ptJD1_plane1_C2[0] << ", " << ptJD1_plane1_C2[1] << ", " << ptJD1_plane1_C2[2] << endl;
        cout << "ptJD2_plane1_C2: " << ptJD2_plane1_C2[0] << ", " << ptJD2_plane1_C2[1] << ", " << ptJD2_plane1_C2[2] << endl;

        Eigen::Vector3d ptJD1_uv2 = H1e_C2 * Eigen::Vector3d(ptJD1_plane1_C2[0], ptJD1_plane1_C2[1], 1);
        Eigen::Vector3d ptJD2_uv2 = H1e_C2 * Eigen::Vector3d(ptJD2_plane1_C2[0], ptJD2_plane1_C2[1], 1);
        cout << "ptJD1_uv2:" << ptJD1_uv2.transpose() << endl;
        cout << "ptJD2_uv2:" << ptJD2_uv2.transpose() << endl;

        cout << "factor1=" << (verifyPtProj_C2[0] - ptJD1_uv2[0]) / (verifyPtProj_C2[1] - ptJD1_uv2[1]) << endl;
        cout << "factor2=" << (verifyPtProj_C2[0] - ptJD2_uv2[0]) / (verifyPtProj_C2[1] - ptJD2_uv2[1]) << endl;
    }
}

const char * usage =
" cmd <option>"
"   1 - homegraphy 单应矩阵转化 \n"
"   2 - cameraModel 相机算法模型\n"
;

static void help()
{
    printf("%s\n", usage);
}

int main(int argc, char *argv[])
{
    int option = 4;
    if (argc == 2) {
        option = atoi(argv[1]);
    }

    switch (option) {
    case 1:
        homegraphy2();
        break;
    case 2:
        cameraModel();
        break;
    case 3:
        ct2dTo3D();
        break;
    case 4:
        ct2dTo3D_geo();
        break;
    case 5:
        c1c2Map();
        break;
    default:
        help();
        break;
    }
    return 0;
}
